#include <ros/ros.h>

/* Include files for showing the image streams */
#include <opencv2/opencv.hpp>
#include <opencv2/highgui/highgui.hpp>
#include <opencv/cxcore.hpp>
#include <cv_bridge/CvBridge.h>
#include <cv_bridge/cv_bridge.h>
#include <opencv2/highgui/highgui.hpp>
#include <image_transport/subscriber_filter.h>
#include <message_filters/subscriber.h>

/* Include sensor message headers */
#include <sensor_msgs/CameraInfo.h>
#include <sensor_msgs/Image.h>
#include <sensor_msgs/image_encodings.h>

/* Include message headers */
#include <sensor_msgs/Image.h>
#include <gyrobots_msgs/HeadUpData.h>

/* Include eigen library */
#include "Eigen/Eigen"
#include <math.h>
#include <time.h>

/* include header for option parsing */
#include <getopt.h>

#ifdef HAVE_GTK
#include <gtk/gtk.h>

/* Platform-specific workaround for #3026: stereo_viewer doesn't close when
 	 closing image window. On platforms using GTK+ we connect this to the
 	 window's "destroy" event so that image_view exits */
static void destroy(GtkWidget *widget, gpointer data) {
  	ros::shutdown();
}
#endif

using namespace std;

/* Define global messages */
gyrobots_msgs::HeadUpData headUpData;
sensor_msgs::Image image;

/* Create image */
bool isValidImage = false;
int img_width = 640;
int img_height = 480;
cv_bridge::CvImageConstPtr src(new cv_bridge::CvImage);
IplImage *canvas = cvCreateImage( cvSize(img_width,img_height), 8, 3 );

/* Utility functions */
int sign(float x) {
	if(x < 0) {
		return -1;
	} else if (x == 0) {
		return 0;
	} else {
		return 1;
	}
}

/* Drawing functions */
void drawLineTest(IplImage *frame, double x, double y, double size, double theta, CvScalar color, int lineWidth) {
	cvLine(frame, cvPoint((int)x,(int)y), cvPoint((int)(x + cos(theta * M_PI/180) * size), 
				(int)(y + sin(theta * M_PI/180) * size)), color, lineWidth, CV_AA, 0);
}

void drawBracket(IplImage *frame, double x, double y, double size, double theta, CvScalar color, int lineWidth) {
	int scale = 7;
	//void cvLine(CvArr* img, CvPoint pt1, CvPoint pt2, CvScalar color, int thickness=1, int lineType=8, int shift=0)
	/* main bracket line */
	cvLine(frame, cvPoint((int)x,(int)y), cvPoint((int)(x + cos(theta * M_PI/180) * size), 
				(int)(y + sin(theta * M_PI/180) * size)), color, lineWidth, CV_AA, 0);

	/* left bracket */
	cvLine(frame, cvPoint((int)x,(int)y), cvPoint((int)(x + sin(theta * M_PI/180) * size/scale), 
				(int)(y + cos(theta * M_PI/180) * size/scale)), color, lineWidth, CV_AA, 0);

	/* right bracket line */
	cvLine(frame, cvPoint((int)(x + cos(theta * M_PI/180) * size), 
					(int)(y + sin(theta * M_PI/180) * size)), 
			cvPoint((int)(x + cos(theta * M_PI/180) * size + sin(theta * M_PI/180) * size/scale), 
					(int)(y + sin(theta * M_PI/180) * size + cos(theta * M_PI/180) * size/scale)),
			color, lineWidth, CV_AA, 0);
}

void drawArrow(IplImage *frame, double x, double y, double size, double theta, CvScalar color, bool filled) { 
	double elongation = 1.5;
	double base = size;
	double height = elongation * size;
	double alpha = atan(2*elongation) * 180/M_PI;
	double beta = theta + alpha - 90;

	CvPoint  ptt[3];
	CvPoint* pt[1];
	int  arr[1];
	pt[0] = &(ptt[0]);
	arr[0] = 3;

	pt[0][0].x = x;	 																			/* point A of triangle */
	pt[0][0].y = y;  
	pt[0][1].x = x + (int)(sin(theta * M_PI/180) * base);  										/* point B of triangle */	
	pt[0][1].y = y + (int)(cos(theta * M_PI/180) * base);
	//pt[0][2].x = x + (int)(sin(theta * M_PI/180) * base/2 + cos(theta * M_PI/180) * height); 	/* point C of triangle */
	//pt[0][2].y = y + (int)(cos(theta * M_PI/180) * base/2 + sin(theta * M_PI/180) * height);
	pt[0][2].x = x + cos(beta * M_PI/180) * height;
	pt[0][2].y = y - sin(beta * M_PI/180) * height;
	ptt[0].x = pt[0][0].x; 
	ptt[0].y = pt[0][0].y;
	ptt[1].x = pt[0][1].x; 
	ptt[1].y = pt[0][1].y;
	ptt[2].x = pt[0][2].x; 
	ptt[2].x = pt[0][2].x; 
				
	/* void cvPolyLine(CvArr* img, CvPoint** pts, int* npts, int contours,
	 * 	int is_closed, CvScalar color, int thickness=1, int lineType=8, int shift=0)
	 * 	usage: http://cboard.cprogramming.com/c-programming/97760-setting-up-opencv.html
	 *
	 * 	void cvFillPoly(CvArr* img, CvPoint** pts, int* npts, int contours, 
	 * 		CvScalar color, int lineType=8, int shift=0)
	 *
	 * 	e.g.cvPolyLine( canvas, pts, npts, 4, 1, CV_RGB(255,0,0), CV_FILLED, 8 );	
	 */
	if(filled) {
		cvPolyLine( frame, pt, arr, 2, 1, CV_RGB(0,0,0), (int)(size/3), 8, 0 );
	} else {
		cvPolyLine( frame, pt, arr, 2, 1, CV_RGB(0,0,0), 1, 8, 0 );
	}
}

void drawBracketWithArrow(IplImage *frame, double x, double y, double size, double theta, CvScalar color, int lineWidth, double current_pos) {
	//void drawBracket(double x, double y, double size, double theta, CvScalar color, int lineWidth) {
	//void drawArrow(double x, double y, double size, double theta, CvScalar color, bool filled) { 

	double arrowSize = 15;
	drawBracket(frame, x, y, size, theta, color, lineWidth);
	drawArrow(frame, x + size/2 - arrowSize/2, y + 1.5 * arrowSize, arrowSize, theta + 90, color, false);
	//drawArrow(x + cos(theta * M_PI/180) * size, y + sin(theta * M_PI/180), arrowSize, theta + 90, color, false);
}


void drawDateAndTime(IplImage *frame, int x, int y, CvScalar color) { 
	// Rendering Text into the image works as follows
	/* Initialize font structure
	 *  void cvInitFont(CvFont* font, int fontFace, double hscale, 
	 *      double vscale, double shear=0, int thickness=1, int lineType=8) 
	 *  http://opencv.willowgarage.com/documentation/drawing_functions.html#puttext
	 */
	/* get time and date */
	time_t rawtime;
	struct tm * timeinfo; /* defined here: http://www.cplusplus.com/reference/clibrary/ctime/tm/ */
	time ( &rawtime );
	timeinfo = localtime ( &rawtime );

	double hscale = 0.4;
	double vscale = 0.4;
	CvFont font;
	cvInitFont(&font, CV_FONT_HERSHEY_SIMPLEX, hscale, vscale, 0, 1, 8); 

	stringstream ss; 

	/* set date */
	ss << setprecision(4) << timeinfo->tm_mon << "-" << timeinfo->tm_mday << "-" << timeinfo->tm_year;
	cvPutText(frame, ss.str().c_str(), cvPoint(x, y), &font, color);

	/* set time */
	ss.str("");
	ss << setprecision(4) << timeinfo->tm_hour << "-" << timeinfo->tm_min << "-" << timeinfo->tm_sec;
	cvPutText(frame, ss.str().c_str(), cvPoint(x, y+10), &font, color);
}

void drawAltimeter(IplImage *frame, double x, double y, double size, CvScalar color, int lineWidth) { }

void drawCourseAndHeading(IplImage *frame, double x, double y, double size, CvScalar color, int lineWidth) { 
	double arrow_size = 20;
	cvLine(frame, cvPoint((int)x,(int)y), cvPoint((int)(x + size), (int)y), color, lineWidth, CV_AA, 0);
	drawArrow(frame, x+size, y-arrow_size/2, arrow_size, 0, CV_RGB(0,0,0), false);
	drawArrow(frame, x, y+arrow_size/2, arrow_size, 180, CV_RGB(0,0,0), false);
}
void drawVerticalSpeed(IplImage *frame, double x, double y, double size) { }
void drawSpeed(IplImage *frame, double x, double y, double size) { }
void drawPitchAndRoll(IplImage *frame, double x, double y, double size, double orientation) { 
	/* Draw circle segment with the ellipse function that allows to define 
	 * start and end angle, as shown on 
	 * http://opencv.willowgarage.com/documentation/python/drawing_functions.html#ellipse
	 */
}

void drawActuatorPositions(double x, double y, double size) { }

/* ROS callback functions */
void imageCallback(const sensor_msgs::ImageConstPtr& frame) {
	image = *frame;
	isValidImage = true;

	/* TODO: render all the data and widgets into the frame in the image callback, 
	 * not the data callback */

	//cvShowImage("Head-up Display", frame_cast);
}

void headUpDataCallback(const gyrobots_msgs::HeadUpData::ConstPtr& data) {
	bool render_votes = false;
	bool render_lines = false;
	bool render_circles = false;
	int scale = 255;
	float alpha, beta;
	int i,j,k,l,color;
	int origin_x = 340;
	int origin_y = 240;

	CvPoint  ptt[4];
	CvPoint* pt[1];
	int  arr[1];
	pt[0] = &(ptt[0]);
	arr[0] = 4;

	/* Reset to white background, so text renderings get updated */
	cvSet(canvas, CV_RGB(255,255,255));

	if(isValidImage) {
		/* Convert from the ROS image message to a CvImage suitable for 
		*  working with OpenCV for processing 
		*/
		cv_bridge::CvImagePtr cv_ptr;

		try {
			/* Always copy, returning a mutable CvImage
			*  OpenCV expects color images to use BGR channel order
			*/
			cv_ptr = cv_bridge::toCvCopy(image, "bgr8");
			src = cv_ptr; 
		} catch (cv_bridge::Exception& e) {
			/* if there is an error during conversion, display it */
			ROS_ERROR("tutorialROSOpenCV::main.cpp::cv_bridge exception: %s", e.what());
			return;
		}

		/* cast images to a format OpenCV can work with */
		IplImage *frame_cast = &static_cast<IplImage>(src->image);

		/* Draw all elements onto frame */
		drawDateAndTime(frame_cast, 40, 40, CV_RGB(0, 0, 0));
		drawCourseAndHeading(frame_cast, 170, 40, 320, CV_RGB(0,0,0), 1);
		drawBracketWithArrow(frame_cast, 100, 420, 100, 0, CV_RGB(0,0,0), 1, 50);

		/* Display image with overlays */
		cvShowImage("Head-up Display", frame_cast);
	}

	//drawArrow(double x, double y, double size, double orientation, CvScalar color, bool filled) { 
	//void drawBracket(double x, double y, double size, double theta, CvScalar color, int lineWidth) {
	//void drawCourseAndHeading(double x, double y, double size, CvScalar color, int lineWidth) { 
	//void drawBracketWithArrow(double x, double y, double size, double theta, CvScalar color, int lineWidth, double current_pos) {
	//drawArrow(200, 200, 40, 45, CV_RGB(0,0,0), false);
	//drawBracket(300, 200, 40, 0, CV_RGB(0,0,0), 2);
	//drawBracket(400, 200, 40, 90, CV_RGB(0,0,0), 1);

	// Drawing a circle works as follows 
	// use color chartreuse: RGB = 127 255 0 
	// see http://kb.iu.edu/data/aetf.html
	//cvCircle(canvas, cvPoint(origin_x + xrender, origin_y + yrender), 5, CV_RGB(127,255,0), 4);

	/* Display the image using OpenCV */
	//cvShowImage("Head-up Display", canvas);
	cv::waitKey(3);
}

int main(int argc, char **argv) {
	ros::init(argc, argv, "head_up_display");

	string head_up_data_topic = "/head_up_display_data";
	string head_up_image_topic = "/mono";

	string help = 
	"\n"
	"Usage: Specify topic that provides the head-up display data\n "
	"\n"
	"\t--help\t			show this help message \n"
	"\t--topic-data   data_topic      specify topic providing head-up display data\n"
	"\t--topic-image  image_topic     specify topic providing head-up image\n"
	"\n"
	"If no option is selected, the node is run with the default parameter \n"
	"--topic-data /head_up_display_data --topic-image /mono\n";


	if(argc == 1) {
		cout << help << endl;
		ROS_INFO("Head-up Display started");
	} else {
		ROS_INFO("Head-up Display started");
	
		// parse command line arguments
		int c;
		static struct option long_options[] =
		{
		{"help",     	required_argument, 0, 'h'},
		{"topic-data",  required_argument, 0, 'd'},
		{"topic-image", required_argument, 0, 'i'},
		{0, 0}
		};
			
		/* getopt_long stores the option index here. */
		int option_index = 0;

		c = getopt_long (argc, argv, "h:d:i", long_options, &option_index);

		while(c != -1) {
			switch (c) {
				case 0:
					break;
				case 'h':
					cout << help << endl;
					break;

				case 'd':
					cout << "Provided topic: " << optarg << endl;
					head_up_data_topic = optarg;
					break;

				case 'i':
					cout << "Provided topic: " << optarg << endl;
					head_up_image_topic = optarg;
					break;

				case '?':
					/* getopt_long already printed an error message. */
					break;

				default:
					abort ();
			}

			c = getopt_long (argc, argv, "h:d:i", long_options, &option_index);
		}
	}

		
	/* Show images in separate windows */
	int flags = CV_WINDOW_AUTOSIZE;
	cv::namedWindow("Head-up Display", flags);

#ifdef HAVE_GTK
	g_signal_connect(GTK_WIDGET( cvGetWindowHandle("Head-up Display") ),
		     "destroy", G_CALLBACK(destroy), NULL);
#endif

	cvStartWindowThread();

	/* Set white background */
	cvSet(canvas, CV_RGB(255,255,255));
	cvShowImage("Head-up Display", canvas);

	/* Create an ImageTransport instance, initializing it with our NodeHandle */
	ros::NodeHandle nh;
	ros::Subscriber head_up_display_sub;
	image_transport::ImageTransport it(nh);

	/* Subscribe to data topic */
	head_up_display_sub = nh.subscribe (head_up_data_topic, 30, &headUpDataCallback);
	ROS_INFO("Head-up Display: Subscribed to topic %s", head_up_data_topic.c_str());

	/* Subscribe to image topic */
	image_transport::Subscriber image_sub = it.subscribe(head_up_image_topic, 1, imageCallback);
	ROS_INFO("Head-up Display: Subscribed to topic %s", head_up_image_topic.c_str());


	/* Publish object info */
	ros::Rate r(25);
	while(ros::ok()) {
		r.sleep();
		ros::spinOnce();
	}

	return 0;
}
